#include "ui_recorder.h"
#include "ui_topic.h"
#include "QtWidgets/QMainWindow"
#include "QDialog"
#include <QDebug>
#include "QFileDialog"
#include "QTime"
#include "iostream"
#include "QApplication"
#include "pcl/visualization/pcl_visualizer.h"
#include "pcl/io/pcd_io.h"
#include "pcl_conversions/pcl_conversions.h"
#include <vtkRenderWindow.h>

#include <unistd.h>            // access
#include <sys/types.h>         // mkdir   rmdir
#include <sys/stat.h>
#include "cv_bridge/cv_bridge.h"
#include "opencv2/opencv.hpp"

#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include "sensor_msgs/Image.h"
class TopicDialog : public QDialog, public Ui::Dialog{

    public:
        TopicDialog(QWidget* parent):QDialog(parent) {
            this->setupUi(this);
        }
    
        QString getLidarTopic(){
            return this->lineEdit->text();
        }
        QString getImageTopic(){
            return this -> lineEdit_2->text();
        }

};


//  接收ROS的消息，并存储到队列中
//  开启一个线程，实时将消息队列中的数据更新到窗口

class Wind : public QDialog{
    public:
    Wind(QWidget* parent=0) : QDialog(parent), ui(new Ui::Form),
    visual(new pcl::visualization::PCLVisualizer("viewer", false)),
    cloud(new pcl::PointCloud<pcl::PointXYZI>),
    comboSig(&QComboBox::currentIndexChanged),
    nh_(""),
    imgType(0){
        image_topic = "/camera/image_raw";
        lidar_topic = "/livox/points";
        dest_dir = "./data";
        ui->setupUi(this);
        initViewer();
        initButtons();
        
        cloud_queue.resize(30);
        for(auto& p : cloud_queue){
            p = pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>);
        }
        head = 0;
        cloud_view_len = 1;
        
    }

    void imgCB(const sensor_msgs::Image::ConstPtr& img_msg){
        cv_bridge::CvImagePtr img;
        if(0==imgType){
            img = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::TYPE_8UC3);
            m = img->image;
            QPixmap pm =  QPixmap::fromImage(
            QImage(m.data, m.cols, m.rows, m.step, QImage::Format_RGB888).rgbSwapped()
        );
        }
        else if(1==imgType){
            img = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::TYPE_8UC1);
            m = img->image;
            QPixmap pm =  QPixmap::fromImage(
            QImage(m.data, m.cols, m.rows, m.step, QImage::Format_Grayscale8).rgbSwapped()
        );
        }else if(2==imgType){
            img = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::TYPE_32FC1);
            m = img->image;
            QPixmap pm =  QPixmap::fromImage(
            QImage(m.data, m.cols, m.rows, m.step, QImage::Format_Grayscale8).rgbSwapped()
        );
        }
        m = img->image;
        QPixmap pm =  QPixmap::fromImage(
            QImage(m.data, m.cols, m.rows, m.step, QImage::Format_RGB888).rgbSwapped()
        );
        ui->imgViewer->setPixmap(pm);
        qDebug() << "get one pic";
    }

    void pclCB(const sensor_msgs::PointCloud2::ConstPtr& pcl_msg){
        pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_ptr(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::fromROSMsg(*pcl_msg, *pcl_ptr);
        *cloud_queue[head] = *pcl_ptr;
        head = (head + 1) % 30;
        qDebug() << "get one cloud";
        updateCloud();
    }
    void updateCloud(){
        cloud->clear();
        int s = head;
        for(int i=0; i < cloud_view_len; i++){
            int ind = (30 + head - i) % 30;
            *cloud += *cloud_queue[ind];
        }
        visual->updatePointCloud<pcl::PointXYZI>(cloud, "cloud");
        ui->cloudViewer->update();
    }
    void initSub(){
        sub_img_ = nh_.subscribe(this->image_topic.toStdString(), 10, &Wind::imgCB, this);
        sub_pcl_ = nh_.subscribe(this->lidar_topic.toStdString(), 10, &Wind::pclCB, this);
    }

    void initViewer(){
        ui->cloudViewer->SetRenderWindow(visual->getRenderWindow());
        visual->setupInteractor(ui->cloudViewer->GetInteractor(), ui->cloudViewer->GetRenderWindow());

        // this->setCentralWidget(ui->cloudViewer);
        // pcl::io::loadPCDFile("/home/amo/CLionProjects/QtPcl/exp_0.pcd", *cloud);
        pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> cc(cloud, "intensity");
        visual->addPointCloud<pcl::PointXYZI>(cloud, cc, "cloud");
        ui->cloudViewer->update();
    }

    void initButtons(){
        ui->pushButton->setText("START");
        connect(ui->pushButton, &QPushButton::clicked, this, &Wind::pbStartAndStop);
        connect(ui->pushButton_2, &QPushButton::clicked, this, &Wind::pbSaveData);
        connect(ui->pushButton_3, &QPushButton::clicked, this, &Wind::pbLocate);
        connect(ui->comboBox, comboSig, this, &Wind::cbPuriod);
        connect(ui->comboBox_2, comboSig, this, &Wind::cbImageType);
        connect(ui->pushButton_4, &QPushButton::clicked, this, &Wind::pbTopic);
    }

    private:
    void pbStartAndStop(){
        static char pb_flag = 2;
        // TODO 提示先选择保存位置
        if(2 == pb_flag){
            ui->pushButton->setText("PAUSE");
            pb_flag = 1;
            initSub();
            // timer = nh_.createTimer(ros::Duration(0.02), [](const ros::TimerEvent&){
            //     ros::spinOnce();
            // });
            timer.setInterval(0.02);
            connect(&timer, &QTimer::timeout, this, &Wind::timerLoop);
            timer.start();
        }
        // 开始后暂停和继续的状态切换
        else if(0==pb_flag){
            ui->pushButton->setText("PAUSE");
            pb_flag = 1;
            timer.start();
        } else if(1==pb_flag){
            ui->pushButton->setText("CONTINUE");
            pb_flag = 0;
            timer.stop();
        }
        qDebug() << "pb is clicked";
    }
    void timerLoop(){
    
        ros::spinOnce();
    }
    void pbSaveData(){
        static int count = 0;
        QString pcd_filename = dest_dir + "/cloud-" + QString::number(count) + ".pcd";
        QString img_filename = dest_dir + "/image-" + QString::number(count) + ".png";
        qDebug() << pcd_filename;
        qDebug() << img_filename;
        if(!m.empty() && !cloud->empty()){
            cv::imwrite(img_filename.toStdString(), m);
            pcl::io::savePCDFile<pcl::PointXYZI>(pcd_filename.toStdString(), *cloud);
        }// 保存数据到指定文件夹
        count ++;
    }
    void pbLocate(){
        QFileDialog fd(this);
        QString dir = fd.getExistingDirectory(nullptr, "caption",".");
        QTime time = QTime::currentTime();
        QString sub_dir = time.toString("hh-mm-ss");
        qDebug() << dir;
        if(dir.isEmpty()) return;
        dest_dir = dir + "/" + sub_dir;
        const char* path = dest_dir.toStdString().c_str();
        qDebug("%s", dest_dir.toStdString().c_str());
        if(0 != mkdir(dest_dir.toStdString().c_str(), 0777)){
            qDebug() << "创建失败，请重新创建" ;
        };
    }
    void pbTopic(){
        TopicDialog td(this);
        td.lineEdit->setText(lidar_topic);
        td.lineEdit_2->setText(image_topic);
        if(QDialog::Accepted==td.exec()){
            image_topic = td.getImageTopic();
            lidar_topic = td.getLidarTopic();
            //  设置话题订阅, 若已有订阅者, 则修改其话题或者删除后重新获取订阅者
            initSub();
            // qDebug("lidar_topic %s", lidar_topic.toStdString().c_str());
            qDebug() << image_topic;
            qDebug() << lidar_topic;
        }
        
    }
    void cbPuriod(int index){
        static int len_vec[5] = {1, 5, 10, 20, 30};
        qDebug() << index;
        cloud_view_len = len_vec[index];
        //  修改数据缓存队列长度
    }

    void cbImageType(int index){
        static QString type_list[] = {"8UC3", "8UC1/Mono8", "16FC1", "32FC1"};
        imgType = index;
        qDebug() << "更改接收图像类型为 :" << type_list[index] ;
    }

    private:
        Ui::Form *ui;
        pcl::visualization::PCLVisualizer* visual;
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
        void (QComboBox::*comboSig)(int index);
        QString lidar_topic;
        QString image_topic;
        QString dest_dir;
        ros::NodeHandle nh_;
        ros::Subscriber sub_pcl_;
        ros::Subscriber sub_img_;
        cv::Mat m;
        std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloud_queue;
        unsigned int head;
        int cloud_view_len;
        QTimer timer;
        uint8_t imgType;
};

int main(int argc, char** argv){
    ros::init(argc, argv, "recorder");
    QApplication app(argc, argv);
    Wind w;
    w.show();
    return app.exec();
}
